package es.uji.viselab.robot.links;

import es.uji.viselab.math.Joints;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.RobotAnswer;
import es.uji.viselab.robot.RobotException;
import es.uji.viselab.robot.links.fanuc.RobotCommunicationABBIRB140;

public class RobotLinkABBIRB140 implements RobotLink {

	public static String MODEL;
	public static int NUM_JOINTS;

	RobotCommunicationABBIRB140 irb140;


	public void connect(String ip, int port) throws RobotException {
		System.out.println("Connecting to robot at " + ip + ":" + port);
		irb140 = new RobotCommunicationABBIRB140();
		irb140.connect(ip, port);
	}

	public Joints getJoints() throws RobotException {
		Joints joints;
		RobotAnswer answer = irb140.send2("get joints");
		joints = new Joints();
		try {
			for (int i = 1; i <= 6; i++) {
				joints.setJoint(i, answer.getVal(i + 1));
			}
		} catch (Exception e) {
			throw new RobotException(
					"Parse error in the answer of the server when calling 'get joints', expected: 'string int int int int int int' got: "
							+ answer.getText());
		}
		return joints;

	}

	public RobotToolLocation getXYZWPR() throws RobotException {
		RobotToolLocation rtl;
		RobotAnswer answer = irb140.send2("get xyz");
		PointXYZ xyz = new PointXYZ();
		PointXYZ wpr = new PointXYZ(0, 0, 0);
		try {
			xyz.setX(answer.getVal(2));
			xyz.setY(answer.getVal(3));
			xyz.setZ(answer.getVal(4));
		} catch (Exception e) {
			throw new RobotException(
					"Parse error in the answer of the server when calling 'get xyz', expected: 'string int int int' got: "
							+ answer.getText());
		}
		rtl = new RobotToolLocation(xyz, wpr);
		return rtl;
	}

	public boolean isConnected() {

		try {
			irb140.ping();
			return true;
		} catch (Exception e) {
			return false;
		}

	}

	public void disconnect() throws RobotException {
		irb140.disconnect();
	}

	public String ping() throws RobotException {
		return irb140.ping();
	}

	public void move(Joints j) throws RobotException {
		String command = "set joints ";
		command = command + (int) j.getJoint1() + " " + (int) j.getJoint2()
				+ " " + (int) j.getJoint3() + " " + (int) j.getJoint4() + " "
				+ (int) j.getJoint5() + " " + (int) j.getJoint6();
		String answer = irb140.send(command);
		if (!answer.equals("OK")) {
			throw new RobotException("Command 'set joints' failed. " + answer);
		}
	}

	public void move(RobotToolLocation rtl) throws RobotException {
		String command = "set xyz ";
		command = command + (int) rtl.getLocation().getX() + " "
				+ (int) rtl.getLocation().getY() + " "
				+ (int) rtl.getLocation().getZ();
		String answer = irb140.send(command);
		if (!answer.equals("OK")) {
			throw new RobotException("Command 'set xyz' failed. " + answer);
		}
	}
}

/*
 * public void setKinectServoControl(boolean value) { kinectServoControl=value;
 * if (value) { try { RobotToolLocation rtl=getXYZWPR();
 * kinectServoControlBasePosion = rtl.getLocation(); } catch (Exception e) { //
 * TODO Auto-generated catch block RobotToolLocation rtl= new
 * RobotToolLocation(500,0,500,0,0,0); kinectServoControlBasePosion =
 * rtl.getLocation(); // e.printStackTrace();
 * 
 * }
 * 
 * } }
 * 
 * public boolean getKinectServoControl() { return kinectServoControl; }
 * 
 * public PointXYZ kinectServoControl(int dx, int dy, int dz) { if
 * (kinectServoControl){ int x = kinectServoControlBasePosion.getX() + dx; int y
 * = kinectServoControlBasePosion.getY() + dy; int z =
 * kinectServoControlBasePosion.getZ() + dz; RobotToolLocation rtl = new
 * RobotToolLocation(x, y, z, 0 , 0, 0); try { move(rtl); } catch
 * (RobotException e) { // TODO Auto-generated catch block e.printStackTrace();
 * } //System.out.println("Move: " +x + " "+ y + " "+ z); return
 * rtl.getLocation(); } return null;
 * 
 * }
 */

